#include "sbus.h"
#include "tim.h"

SBUS_CH_Struct SBUS_CH;

//将sbus信号转化为通道值
uint8_t update_sbus(uint8_t *buf)
{
				//一定保证不解析错误的数据
    if (buf[23] == 0 && buf[0] == 0x0f)
    {
        SBUS_CH.ConnectState = 1;
        SBUS_CH.CH1 = ((int16_t)buf[ 1] >> 0 | ((int16_t)buf[ 2] << 8 )) & 0x07FF;
        SBUS_CH.CH2 = ((int16_t)buf[ 2] >> 3 | ((int16_t)buf[ 3] << 5 )) & 0x07FF;
        SBUS_CH.CH3 = ((int16_t)buf[ 3] >> 6 | ((int16_t)buf[ 4] << 2 ) | (int16_t)buf[ 5] << 10 ) & 0x07FF;
        SBUS_CH.CH4 = ((int16_t)buf[ 5] >> 1 | ((int16_t)buf[ 6] << 7 )) & 0x07FF;
        SBUS_CH.CH5 = ((int16_t)buf[ 6] >> 4 | ((int16_t)buf[ 7] << 4 )) & 0x07FF;
        SBUS_CH.CH6 = ((int16_t)buf[ 7] >> 7 | ((int16_t)buf[ 8] << 1 ) | (int16_t)buf[9] << 9 ) & 0x07FF;
        SBUS_CH.CH7 = ((int16_t)buf[ 9] >> 2 | ((int16_t)buf[10] << 6 )) & 0x07FF;
        SBUS_CH.CH8 = ((int16_t)buf[10] >> 5 | ((int16_t)buf[11] << 3 )) & 0x07FF;
        SBUS_CH.CH9 = ((int16_t)buf[12] << 0 | ((int16_t)buf[13] << 8 )) & 0x07FF;
        SBUS_CH.CH10 = ((int16_t)buf[13] >> 3 | ((int16_t)buf[14] << 5 )) & 0x07FF;
        SBUS_CH.CH11 = ((int16_t)buf[14] >> 6 | ((int16_t)buf[15] << 2 ) | (int16_t)buf[16] << 10 ) & 0x07FF;
        SBUS_CH.CH12 = ((int16_t)buf[16] >> 1 | ((int16_t)buf[17] << 7 )) & 0x07FF;
        SBUS_CH.CH13 = ((int16_t)buf[17] >> 4 | ((int16_t)buf[18] << 4 )) & 0x07FF;
        SBUS_CH.CH14 = ((int16_t)buf[18] >> 7 | ((int16_t)buf[19] << 1 ) | (int16_t)buf[20] << 9 ) & 0x07FF;
        SBUS_CH.CH15 = ((int16_t)buf[20] >> 2 | ((int16_t)buf[21] << 6 )) & 0x07FF;
        SBUS_CH.CH16 = ((int16_t)buf[21] >> 5 | ((int16_t)buf[22] << 3 )) & 0x07FF;
        return 1;
    }
    else
    {
        SBUS_CH.ConnectState = 0;
        return 0;
    }
}
//电机 右
float DutyCycle_Right = 0;
//电机 左
float DutyCycle_Left = 0;
//控制行走流程
void SBUS_CONTROL_FLOW(void)
{
		//接收到sbus数据 通道三数据转换  转换对比值  CH3是开发板PWM1     CH4是开发板PWM2
		DutyCycle_Right = sbus_CH_toM2006Value(SBUS_CH.CH2, (float)PWM_ZKB_MIN ,(float)PWM_ZKB_MAX);
		DutyCycle_Left  = sbus_CH_toM2006Value(SBUS_CH.CH1,(float)PWM_ZKB_MIN,(float)PWM_ZKB_MAX);
		//修改通道2 3 的占空比
		LL_TIM_OC_SetCompareCH3(TIM3,DutyCycle_Right);
		LL_TIM_OC_SetCompareCH4(TIM3,DutyCycle_Left);
}


//将sbus信号通道值转化为特定区间的数值  [p_min,p_max] 
float sbus_to_Range(uint16_t sbus_value, float p_min, float p_max)
{
	  float p;
    p = p_min + (float)(sbus_value - SBUS_RANGE_MIN) * (p_max-p_min)/(float)(SBUS_RANGE_MAX - SBUS_RANGE_MIN);  
    if (p > p_max) p = p_max;
    if (p < p_min) p = p_min;
    return p;
}

//sbus 信号接收 标识数据状态 1为正常
int DMA1_CH6_State = 0;
//数组rc__buff的拷贝
uint8_t rc_buff_copy[25];
//发送给电机的速度值
float speed = 0;

float sbus_CH_toM2006Value(uint16_t CH_num, float p_min, float p_max)
{
	 if(DMA1_CH6_State == 1)
		{
		update_sbus((uint8_t *)rc_buff_copy);
		speed = sbus_to_Range((uint16_t)CH_num,p_min,p_max);
		return speed;
		}
		return 0;
}

extern uint8_t txdata[8];

void sbus_toM2006_ValueDecompose(int16_t Value, uint8_t Value_buff[])
{
		Value_buff[0] = (Value >> 8);
		Value_buff[1] = Value;
} 




